#include "kalman_scan.hpp"


float find_min_exclude_zero(const std::vector<float>& data, int status) {
    static std::array<float, 4> previous_min_non_zero = {std::numeric_limits<float>::max(), 
                                                        std::numeric_limits<float>::max(), 
                                                        std::numeric_limits<float>::max(), 
                                                        std::numeric_limits<float>::max()};

    float min_non_zero = std::numeric_limits<float>::max();

    for (float d : data) {
        if (d > 0 && d < min_non_zero && !std::isinf(d)) {
            min_non_zero = d;
        }
    }
    if (min_non_zero == std::numeric_limits<float>::max()) {
        if (status >= 1 && status <= 4 && previous_min_non_zero[status - 1] != std::numeric_limits<float>::max()) {
            return previous_min_non_zero[status - 1];
        } else {
            ROS_ERROR("return 0 because no non-zero finite value found");
            return 0;
        }
    } else {
        if (status >= 1 && status <= 4) {
            previous_min_non_zero[status - 1] = min_non_zero;
        }
        return min_non_zero;
    }
}

bool isFirst=true;
void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_msg, ros::Publisher &scan_plot_pub,KalmanFilter &kf) {
    // 处理收到的scan消息
    ROS_INFO("Received scan message with %lu ranges", laser_msg->ranges.size());
    std::vector<float> g_ranges_ = laser_msg->ranges;
    float tail_space_raw;
    tail_space_raw = find_min_exclude_zero(std::vector<float>(g_ranges_.begin() + 330, g_ranges_.begin()+ 390), 2);

    if(isFirst)
    {
        kf.setInitial_value(tail_space_raw);
        isFirst = false;
    }
    else{
        double filtered_value = kf.update(tail_space_raw);
        // 创建自定义消息
        function_test::ScanPlot scan_plot_msg;
        scan_plot_msg.raw_data = tail_space_raw;
        scan_plot_msg.filtered_data = filtered_value;

        // 发布数据
        scan_plot_pub.publish(scan_plot_msg);
    }
}

int main(int argc, char** argv) 
{
    // 初始化ROS节点
    ros::init(argc, argv, "rosbag_scan_reader");
    ros::NodeHandle nh;
    ros::Publisher scan_plot_pub = nh.advertise<function_test::ScanPlot>("/scan_plot", 10);

    // 初始化卡尔曼滤波器
    double process_noise = 3e-2; // Q 越小，收敛越明显，趋向一条直线
    double measurement_noise = 5e-1; // R 越小，收敛越快
    double estimation_error = 2;  // P 越大，收敛越快
    double initial_value = 0;

    KalmanFilter kf(process_noise, measurement_noise, estimation_error, initial_value);


    // 打开rosbag文件
    rosbag::Bag bag;
    bag.open("/home/zhangjj/dm_ws/src/function_test/script/scan.bag", rosbag::bagmode::Read);

    // 创建一个view，订阅"/scan"话题
    rosbag::View view(bag, rosbag::TopicQuery("/scan"));

    // 遍历view中的所有消息
    for(rosbag::MessageInstance const m : view) {
        // 处理收到的消息
        sensor_msgs::LaserScan::ConstPtr scan_msg = m.instantiate<sensor_msgs::LaserScan>();
        if(scan_msg != nullptr) {
            scanCallback(scan_msg, scan_plot_pub, kf);
        }
        ros::Duration(0.05).sleep();
        // 检查是否有退出信号
        if (ros::isShuttingDown()) {
            ROS_INFO("收到退出信号，程序即将关闭。");
            break; // 退出for循环
        }
    }

    // 关闭rosbag文件
    bag.close();
    return 0;
}